#!/usr/bin/env python

import rospy
import numpy as np
import rospy
import tf
from tranformation_demo.transformations import *
 

def normalize_quaternion(quat):
    # 转换为 numpy 数组，确保兼容性
    quat = np.array(quat)
    # 计算四元数的模长
    norm = np.linalg.norm(quat)
    # 归一化四元数
    return quat / norm

q_test = [-0.0508, 0.9982, -0.0084, 0.0303]
T_robot = quaternion_matrix(q_test)
R_robot = T_robot[:3,:3]
euler_test = euler_from_matrix(R_robot, 'sxyz')
print("euler_test: ")
print(np.degrees(euler_test))


q0 = [-0.242275, 0.126609, -0.807096, 0.52326 ]
T_robot = quaternion_matrix(q0)
R_robot = T_robot[:3,:3]

euler0 = euler_from_matrix(R_robot, 'rzyx')
print("euler0: ")
print(np.degrees(euler0))

# 绕自身X轴旋转180度
direction = R_robot[:3, 0]
R1 = rotation_matrix(np.radians(180), direction)[:3, :3]
R_robot = np.dot(R1, R_robot)
T_robot = np.eye(4)
T_robot[:3,:3] = R_robot
q1 = quaternion_from_matrix(T_robot)
q1 = normalize_quaternion(q1)
euler1 = euler_from_matrix(R_robot, 'sxyz')
print("euler1: ")
print(np.degrees(euler1))

# 绕自身Z轴旋转90度
direction = R_robot[:3, 2]
R1 = rotation_matrix(np.radians(90), direction)[:3, :3]
R_robot = np.dot(R1, R_robot)
T_robot = np.eye(4)
T_robot[:3,:3] = R_robot
q2 = quaternion_from_matrix(T_robot)
q2 = normalize_quaternion(q2)
euler2 = euler_from_matrix(R_robot, 'sxyz')
print("euler2: ")
print(np.degrees(euler2))

#######方法二
q0 = [-0.242275, 0.126609, -0.807096, 0.52326 ]
T_robot = quaternion_matrix(q0)
R_robot = T_robot[:3,:3]
Unit_X = R_robot[:3, 0]
RX = rotation_matrix(np.radians(180), Unit_X)[:3, :3]
Unit_Z = R_robot[:3, 2]
RZ = rotation_matrix(np.radians(90), Unit_Z)[:3, :3]
R = RX @ RZ 
R_robot = R @ R_robot
euler3 = euler_from_matrix(R_robot, 'sxyz')
print("euler3: ")
print(np.degrees(euler3))

# ############################################@@可视化
rospy.init_node('tf_broadcaster')
br = tf.TransformBroadcaster()


# 发布TF广播消息
rate = rospy.Rate(10)  # 设置发布频率为10Hz
while not rospy.is_shutdown():
    br.sendTransform([0,0,0.5],
                    q0,
                    rospy.Time.now(),
                    "q0",
                    "world")
    
    br.sendTransform([0,0,0.9],
                q1,
                rospy.Time.now(),
                "q1",
                "world")

    br.sendTransform([0,0,1.2],
                q2,
                rospy.Time.now(),
                "q2",
                "world")
        
    rate.sleep()
